#include "DJI_transform.h"
#include <stdlib.h>

union u8_u16
{
    uint8_t u8[2];
	uint16_t u16;
};

union u8_f
{
    uint8_t u8[4];
	float f;
};

static DJI_Status **gpDJI_Status = NULL;
static uint8_t gMotorNum = 0;

static void DJI_Error_Handler(void);

enum DJI_cmd {
    eTimeoutMode,   //timeout模式
    eSetOrigin,     //设置原点
    eCurrentMode,   //电流模式
    eSpeedMode,     //速度模式
    ePosationMode,  //位置模式
};

void DJI_currentDutyMode(DJI_HandleTypeDef* hdji, float CurrentDuty);
void DJI_speedMode(DJI_HandleTypeDef* hdji, float Speed, float CurrentDutyLimit);
void DJI_PosMode(DJI_HandleTypeDef* hdji, float Pos, float SpeedDutyLimit, float CurrentDutyLimit);
uint8_t DJI_getSpeed(DJI_HandleTypeDef* hdji, float *speed);
uint8_t DJI_getPos(DJI_HandleTypeDef* hdji, float *pos);
void DJI_SetOrigin(DJI_HandleTypeDef* hdji);

static struct DJI_pf DJI_pf = 
{
	.currentDutyMode = DJI_currentDutyMode,
    .speedMode = DJI_speedMode,
    .posMode = DJI_PosMode,
	.getSpeed = DJI_getSpeed,
	.getPos = DJI_getPos,
	.setOrigin = DJI_SetOrigin,
};


void DJI_Init(DJI_HandleTypeDef *hdji, UserCAN_HandleTypeDef *hcan, uint8_t ID)
{
	
#ifdef __CAN_H__
    if (hcan->Instance == CAN1)
    {
        USER_CAN1_INIT(hcan);
    }
#ifdef CAN2
    else if (hcan->Instance == CAN2)
    {
        USER_CAN2_INIT(hcan);
    }
#endif
	
#elif defined __FDCAN_H__
	if (hcan->Instance == FDCAN1)
    {
        USER_FDCAN1_INIT(hcan);
    }
    else if (hcan->Instance == FDCAN2)
    {
        USER_FDCAN2_INIT(hcan);
    }
    else if (hcan->Instance == FDCAN3)
    {
        USER_FDCAN3_INIT(hcan);
    }
#endif
    else DJI_Error_Handler();
	
	hdji->status.ID = ID;
    hdji->status.hcan = hcan;
	hdji->pf = &DJI_pf;
}



#ifdef __CAN_H__

void DJI_CAN_Transmit(uint8_t ID, UserCAN_HandleTypeDef *hcan, enum DJI_cmd DJI_cmd, const uint8_t *Data, uint8_t len)
{
    if(len > 8) DJI_Error_Handler();
    if (hcan == NULL) DJI_Error_Handler();

    CAN_TxHeaderTypeDef Can_Tx;
    uint32_t pTxMailbox = 0;
    Can_Tx.StdId = ID | (DJI_cmd << 4) | 0x300;
    Can_Tx.ExtId = 0;
    Can_Tx.IDE = CAN_ID_STD;
    Can_Tx.RTR = 0;
    Can_Tx.DLC = len;

	while(HAL_CAN_GetTxMailboxesFreeLevel(hcan )==0){}
    HAL_CAN_AddTxMessage(hcan, &Can_Tx, Data, &pTxMailbox);
}

#elif defined __FDCAN_H__

	void DJI_CAN_Transmit(uint8_t ID, UserCAN_HandleTypeDef *hcan, enum DJI_cmd DJI_cmd, const uint8_t *Data, uint8_t len)
	{
		if(len > 8) DJI_Error_Handler();
		if (hcan == NULL) DJI_Error_Handler();

		FDCAN_TxHeaderTypeDef CAN_Tx;

		CAN_Tx.Identifier = ID | (DJI_cmd << 4) | 0x300;
		CAN_Tx.IdType = FDCAN_STANDARD_ID;
		CAN_Tx.TxFrameType = FDCAN_DATA_FRAME;
		CAN_Tx.DataLength = len;
		CAN_Tx.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
		CAN_Tx.BitRateSwitch = FDCAN_BRS_OFF;
		CAN_Tx.FDFormat = FDCAN_CLASSIC_CAN;
		CAN_Tx.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
		CAN_Tx.MessageMarker = 0;

		while(HAL_FDCAN_GetTxFifoFreeLevel(hcan)==0){}
		HAL_FDCAN_AddMessageToTxFifoQ(hcan, &CAN_Tx, Data);
	}

#endif

void DJI_RxInit(uint8_t DJINum, DJI_HandleTypeDef **DJIArray)
{
	static bool DJI_RxInitFlag = 0;
	if((DJIArray != NULL)&&(DJI_RxInitFlag==0))
	{
		DJI_RxInitFlag = 1;
		gMotorNum = DJINum;
		gpDJI_Status = (DJI_Status **)malloc(DJINum * sizeof(*gpDJI_Status));
		for(uint8_t i=0;i<DJINum;i++)
		{
			gpDJI_Status[i] = &DJIArray[i]->status;
		}
	}
}

void DJI_currentDutyMode(DJI_HandleTypeDef* hdji, float CurrentDuty)
{
    union u8_u16 sU8_u16;

    if (CurrentDuty > 100)
    {
        CurrentDuty = 100;
    } else if (CurrentDuty < -100)
    {
        CurrentDuty = -100;
    }
    sU8_u16.u16 = CurrentDuty * 100;

    DJI_CAN_Transmit(hdji->status.ID, hdji->status.hcan, eCurrentMode, sU8_u16.u8, 4);
}

void DJI_speedMode(DJI_HandleTypeDef* hdji, float Speed, float CurrentDutyLimit)
{
    union u8_f sU8_f;
    union u8_u16 sU8_u16;
    uint8_t Data[6] = {0};

    if (CurrentDutyLimit > 100)
    {
        CurrentDutyLimit = 100;
    } else if (CurrentDutyLimit < -100)
    {
        CurrentDutyLimit = -100;
    }

    sU8_f.f = Speed;
    Data[0] = sU8_f.u8[0];
    Data[1] = sU8_f.u8[1];
    Data[2] = sU8_f.u8[2];
    Data[3] = sU8_f.u8[3];

    sU8_u16.u16 = CurrentDutyLimit * 100;
    Data[4] = sU8_u16.u8[0];
    Data[5] = sU8_u16.u8[1];

    DJI_CAN_Transmit(hdji->status.ID, hdji->status.hcan, eSpeedMode, Data, 6);
}


void DJI_PosMode(DJI_HandleTypeDef* hdji, float Pos, float SpeedDutyLimit, float CurrentDutyLimit)
{   
    union u8_f sU8_f;
    union u8_u16 sU8_u16;
    uint8_t Data[8] = {0};

    if (CurrentDutyLimit > 100)
    {
        CurrentDutyLimit = 100;
    } else if (CurrentDutyLimit < -100)
    {
        CurrentDutyLimit = -100;
    }

    if (SpeedDutyLimit > 100)
    {
        SpeedDutyLimit = 100;
    } else if (SpeedDutyLimit < -100)
    {
        SpeedDutyLimit = -100;
    }

    sU8_f.f = Pos;
    Data[0] = sU8_f.u8[0];
    Data[1] = sU8_f.u8[1];
    Data[2] = sU8_f.u8[2];
    Data[3] = sU8_f.u8[3];

    sU8_u16.u16 = SpeedDutyLimit * 100;
    Data[4] = sU8_u16.u8[0];
    Data[5] = sU8_u16.u8[1];
    
    sU8_u16.u16 = CurrentDutyLimit * 100;
    Data[6] = sU8_u16.u8[0];
    Data[7] = sU8_u16.u8[1];

    DJI_CAN_Transmit(hdji->status.ID, hdji->status.hcan, ePosationMode, Data, 8);
}


void DJI_SetOrigin(DJI_HandleTypeDef* hdji)
{   
    DJI_CAN_Transmit(hdji->status.ID, hdji->status.hcan, eSetOrigin, NULL, 0);
}

#if defined __FDCAN_H__


uint8_t DJI_Analysis(user_FDCAN_Rx_Struct *FDCAN_Rx_struct)
{
    union u8_f sU8_f = {0};

	if (FDCAN_Rx_struct->FDCAN_RxHeader.IdType != FDCAN_STANDARD_ID)
	{
		return 0;
	}

    if (FDCAN_Rx_struct->FDCAN_RxHeader.DataLength != 8)
	{
		return 0;
	}
	
	if ((FDCAN_Rx_struct->FDCAN_RxHeader.Identifier>>4)!=0x3F)
	{
		return 0;
	}

	uint8_t ID = (FDCAN_Rx_struct->FDCAN_RxHeader.Identifier&0x000f);

    for(uint8_t i=0;i<gMotorNum;i++)
    {
        if (ID == gpDJI_Status[i]->ID)
        {
            sU8_f.u8[0] = FDCAN_Rx_struct->Data[0];
            sU8_f.u8[1] = FDCAN_Rx_struct->Data[1];
            sU8_f.u8[2] = FDCAN_Rx_struct->Data[2];
            sU8_f.u8[3] = FDCAN_Rx_struct->Data[3];
            gpDJI_Status[i]->posation = sU8_f.f;

            sU8_f.u8[0] = FDCAN_Rx_struct->Data[4];
            sU8_f.u8[1] = FDCAN_Rx_struct->Data[5];
            sU8_f.u8[2] = FDCAN_Rx_struct->Data[6];
            sU8_f.u8[3] = FDCAN_Rx_struct->Data[7];
            gpDJI_Status[i]->speed = sU8_f.f;
            
	        gpDJI_Status[i]->posRxFlag = 1;
	        gpDJI_Status[i]->speedRxFlag = 1;

            return ID;
        }
    }
	
	return 0;
}

uint8_t DJI_getSpeed(DJI_HandleTypeDef* hdji, float *speed)
{
	if ((hdji->status.speed == 0)||(hdji->status.speedRxFlag == 0))
	{
		return 0;
	}
	*speed = hdji->status.speed;
	hdji->status.speedRxFlag = 0;
	return 1;
}

uint8_t DJI_getPos(DJI_HandleTypeDef* hdji, float *posation)
{
	if ((hdji->status.posation == 0)||(hdji->status.posRxFlag == 0))
	{
		return 0;
	}
	*posation = hdji->status.posation;
	hdji->status.posRxFlag = 0;
	return 1;
}

#endif


void DJI_Error_Handler()
{
    __disable_irq();
    while (1);
}

